import jason.environment.grid.GridWorldModel;
import jason.environment.grid.Location;
import java.util.ArrayList;

/** class that implements the Model of Domestic Robot application */
public class HouseModel extends GridWorldModel {
    

    // the grid size
    public static final int GSize = 41;
	
	static final Object lock = new Object();
    	
	public ArrayList<Location> roads = new ArrayList<Location>();
	public ArrayList<Integer> roadsBestDist = new ArrayList<Integer>();
	public ArrayList<Location> objectives = new ArrayList<Location>();
	public ArrayList<Integer> myRoad = new ArrayList<Integer>();
	public ArrayList<Long> myTime = new ArrayList<Long>();
	public ArrayList<Double> mySpeed = new ArrayList<Double>();
	public ArrayList<ArrayList<Boolean> > gridInUse = new ArrayList<ArrayList<Boolean> >();
    public HouseModel() {
        // create a 15x15 grid with one mobile agent
        super(GSize, GSize, 99); // MUDA COM No de AGENTES
		
		for(int i = 0; i < GSize; i++){
			gridInUse.add(new ArrayList<Boolean>());
			for(int j = 0; j < GSize; j++){
				gridInUse.get(i).add(false);
			}
		}
		
        // initial location of robot (column 3, line 3)
        // ag code 0 means the robot
		
// START ROADS & AGENTS
		roads.add(new Location(0,0));
		roads.add(new Location(10,0));
		roadsBestDist.add(10);
		for(int i = 0; i <= 10; i++){
			setAgPos(i, i, 0);
			objectives.add(new Location(10,0));
			myRoad.add(0);
			mySpeed.add(0.0);
			myTime.add(System.nanoTime());
			gridInUse.get(i).set(0, true);
		}
		roads.add(new Location(0,5));
		roads.add(new Location(10,5));
		roadsBestDist.add(10);
		for(int i = 0; i <= 10; i++){
			setAgPos(11 + i, i, 5);
			objectives.add(new Location(10,5));
			myRoad.add(2);
			mySpeed.add(0.0);
			myTime.add(System.nanoTime());
			gridInUse.get(i).set(5, true);
		}
		roads.add(new Location(0,10));
		roads.add(new Location(10,10));
		roadsBestDist.add(10);
		for(int i = 0; i <= 10; i++){
			setAgPos(22 + i, i, 10);
			objectives.add(new Location(10,10));
			myRoad.add(4);
			mySpeed.add(0.0);
			myTime.add(System.nanoTime());
			gridInUse.get(i).set(10, true);
		}
// Other Roads
// First Map
/*
		roads.add(new Location(10,0));
		roads.add(new Location(15,5));
		roadsBestDist.add(5);
		roads.add(new Location(10,5));
		roads.add(new Location(15,5));
		roadsBestDist.add(5);
		roads.add(new Location(10,10));
		roads.add(new Location(15,5));
		roadsBestDist.add(5);
		roads.add(new Location(15,5));
		roads.add(new Location(40,5));
		roadsBestDist.add(25);
*/
// Second Map
// /*
		roads.add(new Location(10,0));
		roads.add(new Location(12,2));
		roadsBestDist.add(5);
		roads.add(new Location(12,2));
		roads.add(new Location(15,5));
		roadsBestDist.add(5);
		
		roads.add(new Location(10,5));
		roads.add(new Location(15,5));
		roadsBestDist.add(5);
		roads.add(new Location(10,10));
		roads.add(new Location(15,5));
		roadsBestDist.add(5);
		
		roads.add(new Location(15,5));
		roads.add(new Location(18,2));
		roadsBestDist.add(7);
		roads.add(new Location(18,2));
		roads.add(new Location(20,0));
		roadsBestDist.add(7);

		//New Gamb
		roads.add(new Location(12,2));
		roads.add(new Location(18,2));
		roadsBestDist.add(7);		
		
		
		
		roads.add(new Location(15,5));
		roads.add(new Location(20,5));
		roadsBestDist.add(5);
		roads.add(new Location(15,5));
		roads.add(new Location(20,10));
		roadsBestDist.add(7);
		
		roads.add(new Location(20,0));
		roads.add(new Location(40,0));
		roadsBestDist.add(20);
		roads.add(new Location(20,5));
		roads.add(new Location(40,5));
		roadsBestDist.add(20);
		roads.add(new Location(20,10));
		roads.add(new Location(40,10));
		roadsBestDist.add(20);

// */
/*		Test Roads and Agents
        setAgPos(0, 5, 0);
		objectives.add(new Location(10,5));
        gridInUse.get(5).set(0, true);
		
	    setAgPos(1, 6, 1);
		objectives.add(new Location(10,5));
        gridInUse.get(6).set(1, true);	
		//Adiciona Estradas
		
		roads.add(new Location(5,0));
		roads.add(new Location(10,5));
		roadsBestDist.add(5);
		roads.add(new Location(10,5));
		roads.add(new Location(10,0));
		roadsBestDist.add(5);
		*/
		for(int i=0; i < roads.size(); i = i + 2){
			Location r1 = new Location(roads.get(i).x, roads.get(i).y);
			Location r2 = new Location(roads.get(i+1).x, roads.get(i+1).y);
			add(OBSTACLE, r1);
			while((r1.x != r2.x) || (r1.y != r2.y)){
				if (r1.x < r2.x)        r1.x++;
				else if (r1.x > r2.x)   r1.x--;
				if (r1.y < r2.y)        r1.y++;
				else if (r1.y > r2.y)   r1.y--;
				add(OBSTACLE, r1);
			}
		}
    }
    boolean changeRoad(Integer agente, Integer road) {
		objectives.set(agente, roads.get(road + 1));
		myRoad.set(agente, road);
		Location r1 = new Location(roads.get(road).x, roads.get(road).y);
		Location dest = objectives.get(agente);
        if (r1.x < dest.x)        r1.x++;
        else if (r1.x > dest.x)   r1.x--;
        if (r1.y < dest.y)        r1.y++;
        else if (r1.y > dest.y)   r1.y--;
        synchronized (lock)
        {
			if(!gridInUse.get(r1.x).get(r1.y)){
				gridInUse.get(getAgPos(agente).x).set(getAgPos(agente).y, false);
				setAgPos(agente, r1);
				if(r1.x != 40)
					gridInUse.get(r1.x).set(r1.y, true);
				long tempo = System.nanoTime();
				mySpeed.set(agente, ((double)(tempo) - (double)myTime.get(agente)) / 1000000000);
				myTime.set(agente, tempo);
			}
		}
		return true;
	}
	
	Integer recFindBackCar(Integer agente){
		return recFindBackCar(agente, 4);
	}

	Integer recFindBackCar(Integer agente, int depth){
		if(depth == 0)
			return -1;

		Location r1 = getAgPos(agente);
		Location dest = roads.get(myRoad.get(agente));
		Integer retorno = -1;

		if (r1.x <= dest.x)        r1.x++;
		else if (r1.x >= dest.x)   r1.x--;

		for(int i = 0; i < objectives.size(); i++){
			Location r2 = getAgPos(i);
			if ( r1.x == r2.x && r1.y == r2.y)
				return i;
		}

		if (r1.y <= dest.y)        r1.y++;
		else if (r1.y >= dest.y)   r1.y--;

		for(int i = 0; i < objectives.size(); i++){
			Location r2 = getAgPos(i);
			if ( r1.x == r2.x && r1.y == r2.y)
				return i;
		}

		return recFindBackCar(agente, depth-1);
	}

	Integer findBackCar(Integer agente){
		Location r1 = getAgPos(agente);
		Location dest = roads.get(myRoad.get(agente));
		Integer retorno = -1;
		int turn = 0;
		while(retorno == -1 && turn < 4){
			if (r1.x <= dest.x)        r1.x++;
			else if (r1.x >= dest.x)   r1.x--;
			if (r1.y <= dest.y)        r1.y++;
			else if (r1.y >= dest.y)   r1.y--;

			for(int i = 0; i < objectives.size(); i++){
				Location r2 = getAgPos(i);
				if ( r1.x == r2.x && r1.y == r2.y)
					return i;
			}

			turn++;
		}
		return -1;
	}
	
    boolean moveTowards(Integer agente) {
        Location r1 = getAgPos(agente);
		Location dest = objectives.get(agente);
		Boolean retorno = false;
        if (r1.x < dest.x)        r1.x++;
        else if (r1.x > dest.x)   r1.x--;
        if (r1.y < dest.y)        r1.y++;
        else if (r1.y > dest.y)   r1.y--;
        synchronized (lock)
        {
			if(!gridInUse.get(r1.x).get(r1.y)){
				gridInUse.get(getAgPos(agente).x).set(getAgPos(agente).y, false);
				setAgPos(agente, r1);
				if(r1.x != 40)
					gridInUse.get(r1.x).set(r1.y, true);
				retorno = true;
				long tempo = System.nanoTime();
				mySpeed.set(agente, ((double)(tempo) - (double)myTime.get(agente)) / 1000000000);
				myTime.set(agente, tempo);
			}
		}
		//if(retorno)
			//try { Thread.sleep(5000); } catch (InterruptedException e) {}
		return retorno;
        // repaint the fridge and owner locations
/*        if (view != null) {
            view.update(lFridge.x,lFridge.y);
            view.update(lOwner.x,lOwner.y);
        }*/
        //return true;
    }
}
